An Improved MbICP Algorithm for Mobile Robot Pose Estimation

This paper presents an improved version of the metric-based iterative closest point algorithm to estimate robot poses by matching 2D laser scans with different overlapping percentages. Because of the greatly varied density distribution of realistic point clouds, a resampling method is used to accelerate the iteration process and protect the calculation of the rejection threshold from being distorted by reducing dense but unhelpful points. A new procedure that combines point-to-point and point-to-line metrics is used to determine the correct correspondence between partially overlapping scans, which maintains both efficiency and accuracy. In addition, a rejection threshold that is based on the MAD-from-median method is utilized to discard correspondences with large distances, which are likely to be incorrect. Experiments show that the improved algorithm is more accurate and robust than the standard algorithm with respect to the existence of non-overlapping areas, and testing demonstrates that it is valid in practice.